%test program for CST element
function PatchTestT3_2

global e nu H ft

e = 2;
nu = 0.3;
H = -1;
ft = 0.91;

node = [    0 0;
            1 0;
            1 1;
            0 1 ];
element = [ 1 2 3;
            1 3 4   ];

numnode = size(node,1);
free_dofs = [6 8];
prescribed_dofs = [3 5];
num_free_dofs = size(free_dofs(:),1);
num_prescribed_dofs = size(prescribed_dofs(:),1);
Fext = zeros(num_free_dofs,1);
        
KtSystem = computeKt(node,element);

du = 0.5;
up0 = du*ones(num_prescribed_dofs,1);
dlambda = 0.1;
numstep = 10;
lambda = 0;
Usystem = zeros(2*numnode,1);
for i=1:numstep
    lambda = lambda+dlambda;
    up = lambda*up0;
    Kuu = KtSystem(free_dofs,free_dofs);
    Kup = KtSystem(free_dofs,prescribed_dofs);
    u = Kuu\(Fext-Kup*up);
    Usystem(free_dofs) = u;
    Usystem(prescribed_dofs) = up;
    Usystem
end

end

function y = computeKt(node,element)
ne = size(element,1);
nn = size(node,1);
y = zeros(2*nn,2*nn);
for i = 1:ne
    n = element(i,:);
    X1 = node(n(1),:);
    X2 = node(n(2),:);
    X3 = node(n(3),:);
    Kt = computeKtCST(X1(1),X1(2),X2(1),X2(2),X3(1),X3(2));
    con = [2*n(1)-1 2*n(1) 2*n(2)-1 2*n(2) 2*n(3)-1 2*n(3)];
    y(con,con) = y(con,con) + Kt;
end
end

function y = computeRi(node,element)

end

function y = computeKtCST(x1, y1, x2, y2, x3, y3)
global e nu H ft
C = e/(1-nu*nu)*...
    [   1 nu 0;
        nu 1 0;
        0 0 (1-nu)/2    ];
A = computeA(x1, y1, x2, y2, x3, y3);
B = computeB(x1, y1, x2, y2, x3, y3);
y = A*B'*C*B;
end

function A = computeA(x1, y1, x2, y2, x3, y3)
A = 0.5 * abs(det([1 1 1; x1 x2 x3; y1 y2 y3]));
end

function B = computeB(x1, y1, x2, y2, x3, y3)
A = computeA(x1, y1, x2, y2, x3, y3);
B = [  (y2-y3)   0      (y3-y1) 0       (y1-y2)  0;
       0        (x3-x2)  0      (x1-x3)  0       (x2-x1);
       (x3-x2)  (y2-y3) (x1-x3) (y3-y1) (x2-x1)  (y1-y2) ];
B = 1/(2*A).*B;
end

function y = computeStrain(X,U)
B = computeB(x(1), X(2), X(3), X(4), X(5), X(6));
y = B'*U;
end
